#coding=utf-8
import cv2
import numpy as np
import struct
import serial
import time

com = serial.Serial(
        port="/dev/ttyTHS1",
        baudrate=115200,
        bytesize=serial.EIGHTBITS,
        parity=serial.PARITY_NONE,
        stopbits=serial.STOPBITS_ONE,
)

time.sleep(1)

def send_data(Cx):
    tmp=struct.pack("<BhB", 0x2C, int(Cx), 0x5B)
    com.write(tmp)

# 初始化摄像头
cap = cv2.VideoCapture(1)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

ROI_SATRT = (0, 240)
ROI_END = (640, 240)

scan_line_y = 240

left_edge_x = 0
right_edge_x = 0

center_point_x = 0

flag_single_R = 0
flag_single_L = 0

flag_serial = 0

while True:
    ret, frame = cap.read()
    if not ret:
        break
    cv2.rectangle(frame, ROI_SATRT, ROI_END, (255, 255, 0), 1)

    # 转换为灰度图（关键步骤）
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    height, width = gray.shape

    # 高斯滤波
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)

    # 使用开运算去除小的噪声点
    kernel_open = np.ones((3, 3), np.uint8)
    opened = cv2.morphologyEx(blurred, cv2.MORPH_OPEN, kernel_open)
    
    # 使用闭运算连接可能被断开的黑线
    kernel_close = np.ones((5, 5), np.uint8)
    closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, kernel_close)

    # 二值化
    _, binary_image = cv2.threshold(closed, thresh=100, maxval=255, type=cv2.THRESH_BINARY)
    line_data = binary_image[scan_line_y, :]
    
    # scan from middle to left
    for x in range(int((width-1) / 2), 0, -1):
        y1 = 255 - line_data[x]    # 反转Y轴方向
        y2 = 255 - line_data[x - 1]

        if y2 - y1 == 255:
            left_edge_x = x
            flag_single_L = 1
            break

    # scan from middle to right
    for x in range(int((width-1) / 2), width-1, 1):
        y1 = 255 - line_data[x]    # 反转Y轴方向
        y2 = 255 - line_data[x + 1]
        if y2 - y1 == 255:
            right_edge_x = x
            flag_single_R = 1

    # go straight
    if(flag_single_R + flag_single_L == 2):
        center_point_x = int((left_edge_x + right_edge_x) / 2)
        flag_serial = 1
        cv2.circle(frame, (center_point_x, scan_line_y), 5, (255, 0, 255), -1)
        right_edge_x = left_edge_x = 0
        flag_single_R = flag_single_R = 0
   
    # right deviate
    elif flag_single_R == 1:
        left_edge_x = 0
        center_point_x = int((left_edge_x + right_edge_x) / 2)
        flag_serial = 1
        cv2.circle(frame, (center_point_x, scan_line_y), 5, (255, 0, 0), -1)
        right_edge_x = left_edge_x = 0
        flag_single_R = flag_single_R = 0

    # left deviate
    elif flag_single_L == 1:
        right_edge_x = 255
        center_point_x = int((left_edge_x + right_edge_x) / 2)
        flag_serial = 1
        cv2.circle(frame, (center_point_x, scan_line_y), 5, (255, 255, 100), -1)
        right_edge_x = left_edge_x = 0
        flag_single_R = flag_single_R = 0

    if flag_serial == 1:
        flag_serial = 0
        send_data(center_point_x)

    # 在原图画扫描线
    cv2.line(frame, (0, scan_line_y), (width, scan_line_y), (0,255,255), 1)

    cv2.imshow('Camera', frame)

    cv2.moveWindow('Camera', 0, 0)
    
    if cv2.waitKey(1) == 27:  # ESC退出
        break

cap.release()
cv2.destroyAllWindows()
